/*
 * pi_msg.cpp
 *
 *  Created on: May 25, 2021
 *      Author: lu
 */

#include "pi_msg.hpp"
#include "string.h"


const unsigned char header[2]  = {0x55, 0xaa};
const unsigned char ender[2]   = {0x0d, 0x0a};
/**************************************************************************
函数功能：计算八位循环冗余校验，被usartSendData和usartReceiveOneData函数调用
入口参数：数组地址、数组大小
返回  值：无
**************************************************************************/
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
	unsigned char crc;
	unsigned char i;
	crc = 0;
	while(len--)
	{
		crc ^= *ptr++;
		for(i = 0; i < 8; i++)
		{
			if(crc&0x01)
                crc=(crc>>1)^0x8C;
			else
                crc >>= 1;
		}
	}
	return crc;
}
/**********************************END***************************************/



RASPBERRYPI raspberrypi(USART1,(char *)"raspberrypi");
void USART1_IRQHandler(void)
{//串口的中断回调函数
//判断串口4是否空闲
	if(LL_USART_IsActiveFlag_IDLE(USART1))
	{
		LL_USART_ClearFlag_IDLE(USART1);
		LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_5);
		LL_DMA_ClearFlag_DME5(DMA2);
		LL_DMA_ClearFlag_HT5(DMA2);
		LL_DMA_ClearFlag_TC5(DMA2);
		LL_DMA_ClearFlag_TE5(DMA2);
		LL_DMA_ClearFlag_FE5(DMA2);
		raspberrypi.RxDataSize = PIMSG_RX_LEN - LL_DMA_GetDataLength(DMA2, LL_DMA_STREAM_5);
		do{
			if(raspberrypi.RxRawDat[0] != 0x55 || raspberrypi.RxRawDat[1] != 0xaa||(raspberrypi.RxRawDat[2]+6) != raspberrypi.RxDataSize) break;
			if(raspberrypi.LockRx == HAL_LOCKED) break;
			raspberrypi.LockRx = HAL_LOCKED;
			memcpy(raspberrypi.RxDat, raspberrypi.RxRawDat, raspberrypi.RxDataSize);
			raspberrypi.RxDat[raspberrypi.RxDataSize] = 0;
			raspberrypi.LockRx = HAL_UNLOCKED;
			raspberrypi.RxFlag = true;   //收到完整一帧

			BaseType_t YieldRequired = xTaskResumeFromISR(raspberrypiReceiveTaskHandle);
			if(YieldRequired==pdTRUE)
			{
				/*如果函数xTaskResumeFromISR()返回值为pdTRUE，那么说明要恢复的这个
				任务的任务优先级等于或者高于正在运行的任务(被中断打断的任务),所以在
				退出中断的时候一定要进行上下文切换！*/
				portYIELD_FROM_ISR(YieldRequired);
			}
		}while(0);



		LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_5, PIMSG_RX_LEN);
		LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_5);
	}
}

void DMA2_Stream5_IRQHandler(void)
{

}

void DMA2_Stream7_IRQHandler(void)
{
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);
	LL_DMA_ClearFlag_TC7(DMA2);
	raspberrypi.TxFlag = false;

}

void RASPBERRYPI::raspberrypi_Init(void)
{
	LockRx = HAL_UNLOCKED;
	RxFlag = false;
	TxFlag = false;
	RxDataSize = 0;
	executionTime_us = 0;
	Sta = STA_INI;
	Err = ERR_NONE;

	osDelay(400);

	/* 配置接收DMA */
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_5);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_5, (uint32_t)&huart->RDR);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_5, (uint32_t)RxRawDat);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_5, PIMSG_RX_LEN);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_5);
	/* 配置接收DMA */

	/* 配置发送DMA */
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);
	LL_DMA_SetPeriphAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)&huart->TDR);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)TxDat);
	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_7, PIMSG_TX_LEN);
	LL_DMA_ClearFlag_TC7(DMA2);
	LL_DMA_EnableIT_TC(DMA2, LL_DMA_STREAM_7);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_7);
	/* 配置发送DMA */

	LL_USART_EnableDMAReq_RX(huart);
	LL_USART_EnableDMAReq_TX(huart);
	LL_USART_ClearFlag_IDLE(huart);
	LL_USART_EnableIT_IDLE(huart);

	Sta = STA_RUN;

}

bool RASPBERRYPI::raspberrypi_Uart_Send_DMA(uint8_t * pData,uint16_t Size)
{
	if(TxFlag == true) return false;	//串口发送忙,放弃发送该帧数据
	LL_DMA_DisableStream(DMA2,LL_DMA_STREAM_7);

	LL_DMA_SetDataLength(DMA2, LL_DMA_STREAM_7, Size);
	LL_DMA_SetMemoryAddress(DMA2, LL_DMA_STREAM_7, (uint32_t)pData);
	LL_DMA_EnableStream(DMA2, LL_DMA_STREAM_7);
	TxFlag = true;
	return true;
}

void RASPBERRYPI::raspberrypi_Uart_Send(void)
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;


	TxDat[0] = 'i';
	TxDat[1] = 'a';
	TxDat[2] = '\r';
	raspberrypi_Uart_Send_DMA((u8 *)TxDat, 3);


	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;

}


void RASPBERRYPI::raspberrypi_Uart_Receive_Update(void)
{
	startTimerLast = startTimer;
	getTimer_us(&startTimer);
	cycleTime_us = startTimer - startTimerLast;
	if(RxFlag == false) return;
	RxFlag = false;
	if(LockRx == HAL_LOCKED) return;
	LockRx = HAL_LOCKED;
	static unsigned char checkSum             = 0;

	do 
	{
		uint8_t num=RxDat[2];
		checkSum = getCrc8(RxDat, 3 + num);
		// 检查信息校验值
		if(checkSum != RxDat[num+3])break;		//和校验 判断sum
		for(uint8_t i=0;i<7;i++)
		{
			memcpy(temp.data,&RxDat[3+i*8],8);
			mocapPos.mocapPos[i] = temp.d;
		}
		mocapPos.timestamp = startTimer;
		xQueueOverwrite(queueMOCAP,&mocapPos);
			
	}while(0);

	LockRx = HAL_UNLOCKED;

	getTimer_us(&stopTimer);

	executionTime_us = stopTimer - startTimer;

}

extern "C" void raspberrypi_main(void *argument)
{
	raspberrypi.raspberrypi_Init();
	osDelay(10);
	for(;;)
	{
		osSemaphoreAcquire(semRaspberrypi,0xffffffff);
		raspberrypi.raspberrypi_Uart_Send();
	}
}

extern "C" void raspberrypiReceive_main(void *argument)
{
	raspberrypi.raspberrypi_Init();
	osDelay(500);//等待系统完成初始化
	for(;;)
	{
		vTaskSuspend(raspberrypiReceiveTaskHandle);
		raspberrypi.raspberrypi_Uart_Receive_Update();
		led2_Tog;

	}
}


